#include "modules/CtrlModule/CtrlModule.h"
#include "modules/AssistModule/LogModule.h"
#include "modules/AssistModule/TimeModule.h"
#include "data_struct/base/TimeWorkBase/ModuleBase.h"
#include "Communication/InterruptAndRecoveryCom.h"

#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "modules/CtrlModule/ActivePlanner/ObsFollowPlanner.h"
#include "modules/CtrlModule/ActivePlanner/SelfTestPlanner.h"
#include "modules/CtrlModule/ActivePlanner/NaviPlanner.h"
#include "modules/CtrlModule/ActivePlanner/MapPoseCheckPlan.h"
#include "modules/CtrlModule/ActivePlanner/ZigzagPlanner.h"
#include "modules/CtrlModule/ActivePlanner/VirtualObsFollowPlanner.h"
#include "modules/CtrlModule/ActivePlanner/RoomDividerPlanner.h"

#include "modules/CtrlModule/PassivePlanner/CliffReactionPlanner.h"
#include "modules/CtrlModule/PassivePlanner/BumperPlanner.h"
#include "modules/CtrlModule/PassivePlanner/SawtoothWallPlanner.h"
#include "modules/CtrlModule/PassivePlanner/TopCapObsPlanner.h"
#include "modules/CtrlModule/PassivePlanner/TiltReactionPlanner.h"
#include "modules/CtrlModule/PassivePlanner/SlipReactionPlanner.h"
#include "modules/CtrlModule/PassivePlanner/SmallObsPlanner.h"
#include "modules/CtrlModule/PassivePlanner/PostionStuckPlanner.h"
#include "modules/CtrlModule/PassivePlanner/BrushWindedPlanner.h"
#include "modules/CtrlModule/PassivePlanner/FanBlockPlanner.h"
#include "modules/CtrlModule/PassivePlanner/FrontPushableObsPlanner.h"

#include "modules/MonitorModule/VacantMonitor.h"
#include "modules/MonitorModule/TopCapObsMonitor.h"
#include "modules/MonitorModule/SawtoothWallMonitor.h"
#include "modules/MonitorModule/TiltMonitor.h"
#include "modules/MonitorModule/SlipMonitor.h"
#include "modules/MonitorModule/SmallObsMonitor.h"
#include "modules/MonitorModule/PostionStuckMonitor.h"
#include "modules/MonitorModule/BrushWindedMonitor.h"
#include "modules/MonitorModule/FanBlockMonitor.h"
#include "modules/MonitorModule/FrontPushableObsMonitor.h"
#include "modules/MonitorModule/SelfTestMonitor.h"

namespace behavior_controller
{

CtrlModule::CtrlModule(const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_,
                       std::shared_ptr<tf2_ros::Buffer> &tf_,
                       std::shared_ptr<IpaRoomPlanning> ipa_room_plan_)
        : ros_node_(ros_node), tf_buffer(tf_)
{
    controllerPtr = std::make_shared<PurePursuitControllerD>(ros_node_, tf_buffer);
    // 速度设置
    vel_pub_mod.SetNode(ros_node_);
    // 标签位姿设置
//    mark_pose_pub_mod.SetNode(ros_node_);
    // 地图位置检查规划器
//    map_pose_check_planner.SetNode(ros_node_, ipa_room_plan_);
    // 导航规划器
    navi_planner.SetNode(ros_node_, lock_, ipa_room_plan_);
    // 转弯规划器
    zigzag_planner.SetNode(ros_node_, ipa_room_plan_);
    // 房屋分频器
    room_divider_planner.SetNode(ros_node_, ipa_room_plan_);
    //
    insert_pose_client.SetNode(ros_node_);
}

void CtrlModule::UpdateTime2Hz(const evt::Timer2Hz &evt)
{
    //LogPub::Info("behavior_controller", "[CtrlModule] UpdateTime2Hz");
}

void CtrlModule::UpdateTime100Hz(const evt::Timer100Hz &evt)
{
    //-------------------------测试时间频率发布要注释掉下面的代�?-------------------------//
    double linear_vel = 0, angular_vel = 0;
    CtrlStateUpdate(); //主动、�??动�?�划决策
//    std::cout << "ctrl_state= " << DataSet::ctrl_state
//              << " clean_mode= " << DataSet::clean_mode
//              << " ctrl_state_sub= " << DataSet::ctrl_state_sub
//              << " SELT_TEST_RESULT: " << SELT_TEST_RESULT << std::endl;
    if (!SELT_TEST_RESULT)
    {
        ZeroSpeed(linear_vel, angular_vel);
//        std::cout << "self test failed ,wait here ! " << std::endl;
        vel_pub_mod.VelPub(linear_vel, angular_vel);
        return;
    }
    //-------------------------map pose check palnner-------------------------//
    if (DataSet::ctrl_state == MAP_POSE_CHECK_PLAN)
    {
        std::cout << "map_pose_check : running !" << std::endl;
        map_pose_check_planner.run(linear_vel, angular_vel);
        if (map_pose_check_planner.is_finish())
        {
            DataSet::ctrl_state = ROOM_DIVIDER_PLAN;
            if (map_pose_check_planner.is_online())
            {
                room_divider_planner.setOnline(true);
                obs_follow_planner.setOnline(true);
            }
            else
            {
                room_divider_planner.setOnline(false);
                obs_follow_planner.setOnline(false);
            }
            std::cout << "map_pose_check  finished!" << std::endl;
            //start 测试，用后删�?
            ////注释1，测试沿墙，用时关闭注释2
//            updateState(FOLLOW_MODE, OBS_FOLLOW, -1);
//            obs_follow_planner.start_pose.pose.position.x = 1000;
            //end   测试，用后删�?
        }
        ZeroSpeed(linear_vel, angular_vel);
    }
    //-------------------------房间分割 -------------------------//
    else if (DataSet::ctrl_state == ROOM_DIVIDER_PLAN)
    {
        std::cout << "room_divider_planner : running !" << std::endl;
        bool finished = false;
//        finished = true;
        room_divider_planner.run(linear_vel, angular_vel, finished);
        if (finished)
        {
            ZeroSpeed(linear_vel, angular_vel);

            if (FOLLOW_WALL)
            {
                updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN);
                navi_planner.setMission(TO_FOLLOW_POINT_AUTO, geometry_msgs::msg::PoseStamped());
                std::cout << "room_divider_planner finished , next  NAVI_TO_POSE_PLAN" << std::endl;
            }
            else
            {
                updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
                std::cout << "room_divider_planner finished , next  COVERAGE_PLAN" << std::endl;
            }

        }
    }
    //-------------------------navigation -------------------------//
    else if (DataSet::ctrl_state == NAVI_ACTION)
    {
        //-------------------------navigation :NAVI_TO_POSE_PLAN -------------------------//
        if (DataSet::ctrl_state_sub == NAVI_TO_POSE_PLAN)
        {
            bool finished = false;
            std::cout << "navi : running !" << std::endl;
            navi_planner.run(linear_vel, angular_vel, finished);
            if (!finished) return;
            else
            {
                std::cout << "navi to pose : finished !" << std::endl;
                ZeroSpeed(linear_vel, angular_vel);
                if (navi_planner.getMission() < TO_SECTION_POINT)
                {
                    updateState(FOLLOW_MODE, OBS_FOLLOW, -1);
                    obs_follow_planner.start_pose = navi_planner.goal_pose;
                }
                else
                {
                    if (navi_planner.getMission() == TO_SECTION_POINT)
                    {
                        std::cout << "next: COVERAGE_PLAN,SECTION!" << std::endl;
                        updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
                    }
                    else if (navi_planner.getMission() == TO_NEXT_ROOM_POINT)
                    {
                        std::cout << "next: next room TO_FOLLOW_POINT_AUTO!" << std::endl;
                        if (FOLLOW_WALL)
                        {
                            navi_planner.setMission(TO_FOLLOW_POINT_AUTO, geometry_msgs::msg::PoseStamped());
                            updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN);
                        }
                        else
                        {
                            updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
                        }
                    }
                    else if (navi_planner.getMission() == TO_STATION_POINT)
                    {
                        std::cout << "to charging stations!" << std::endl;
                    }
                }
            }
        }
            //-------------------------navigation :COVERAGE_PLAN-------------------------//
        else if (DataSet::ctrl_state_sub == COVERAGE_PLAN)
        {
            int finished_id = UNFINISHED;
            geometry_msgs::msg::PoseStamped goal_pose;
            std::cout << "zigzag : running !" << std::endl;
            zigzag_planner.run(finished_id, linear_vel, angular_vel, goal_pose);
            if (finished_id == UNFINISHED)
            {
                std::cout << "zigzag : UNFINISHED !" << std::endl;
            }
            else if (finished_id == FINISHED)
            {
                ZeroSpeed(linear_vel, angular_vel);
                if (zigzag_planner.hasRepairLeak())
                {
                    std::cout << "zigzag : UNFINISHED !" << std::endl;
                }
                else
                {
                    updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN, WAIT_TIME, WAIT_TIME, WAIT_TIME);
                    navi_planner.setMission(TO_STATION_POINT, goal_pose);
                    std::cout << "zigzag : finished!" << std::endl;
                }
            }
            else
            {
                if (finished_id == OVER_SECTION)
                {
                    std::cout << "next: OVER_SECTION!" << std::endl;
                    updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN,
                                NAVI_MODE, DataSet::ctrl_state, DataSet::ctrl_state_sub);
                    navi_planner.setMission(TO_SECTION_POINT, goal_pose);
                } else if (finished_id == OVER_ROOM)
                {
                    ZeroSpeed(linear_vel, angular_vel);
                    updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN);
                    navi_planner.setMission(TO_NEXT_ROOM_POINT, goal_pose);
                    std::cout << "next: OVER_ROOM!" << std::endl;
                }
            }
        }
    }
    //-------------------------沿边程序，依赖硬件，无法验证-------------------------//
    //单沿边传感器原理：通过传感器数值调整在离�??0.005~0.025m的范围内时，控制角速度缓慢变化，可以达到肉眼�?�察不到的波�?轨迹
    else if (DataSet::ctrl_state == OBS_FOLLOW)
    {
        std::cout << "obs follow : running !" << std::endl;
        bool finished = false;
        obs_follow_planner.run(linear_vel, angular_vel, finished, controllerPtr);
        if (finished && DataSet::clean_mode == FOLLOW_MODE)
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(NAVI_MODE, NAVI_ACTION, COVERAGE_PLAN);
            std::cout << "follow mode finished , next navigation-> zigzag plan " << std::endl;
        }
    }
    //-------------------------沿悬崖程序，依赖硬件，无法验证-------------------------//
    //悬崖响应原理：先判�?导航、还�?沿边模式，沿边模式进入沿悬崖崖逻辑(CliffFollowPlanner)，�?�航模式进入后退，调整方向逻辑(CliffRactionPlanner)
    //沿悬崖原理：通过获取3�?触发Cliff的位�?坐标来�?�算�?崖边界直线，当尚�?检测到�?崖直线时，通过后退�?向来触发�?崖点，当找到�?崖直线后，使机器人朝向和�?崖线平�?�的角度行走�?
    else if (DataSet::ctrl_state == CLIFF_FOLLOW)
    {
        if (!cliff_follow_planner.finished) // 沿悬崖还�?结束的情�?
        {
            bool finished = false;
            std::cout << "cliff_follow : running !" << std::endl;
            cliff_follow_planner.run(linear_vel, angular_vel, finished);
            if (finished && DataSet::clean_mode == NAVI_MODE && DataSet::ctrl_state_sub == COVERAGE_PLAN)
            {
                ZeroSpeed(linear_vel, angular_vel);
                DataSet::ctrl_state = NAVI_ACTION;
                std::cout << "cliff around finished , next navigation-> zigzag plan " << std::endl;
                return;
            }
        }
        if (cliff_follow_planner.finished)  // 沿悬崖结束的情况
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(FOLLOW_MODE, OBS_FOLLOW, -1);
//            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
//                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "cliff_follow : finished !" << std::endl;
        }
    }
        //-------------------------悬崖响应程序，依赖硬件，无法验证-------------------------//
    else if (DataSet::ctrl_state == CLIFF_ACTION)
    {
        if (!cliff_reaction_planner.finished) // 沿悬崖还�?结束的情�?
        {
            std::cout << "cliff_reaction : running !" << std::endl;
            cliff_reaction_planner.run(linear_vel, angular_vel);
        }
        if (cliff_reaction_planner.finished) // 沿悬崖结束的情况
        {
            ZeroSpeed(linear_vel, angular_vel);
            if (DataSet::ctrl_state_suspend == NAVI_ACTION && DataSet::ctrl_state_sub_suspend == NAVI_TO_POSE_PLAN)
            {
                while (rclcpp::ok() &&
                       !insert_pose_client.async_send_request(
                               std::string("CliffReactionPlanner"), 0, false))
                {
                    std::cout << "CliffReactionPlanner: start navigation failed, waiting ..." << std::endl;
                }
            }
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend, DataSet::ctrl_state_sub_suspend,
                        -1, -1, -1);
            std::cout << "cliff_reaction : finished !" << std::endl;
        }
    }
        //-------------------------碰撞传感器响应程序，依赖硬件，无法验证-------------------------//
    else if (DataSet::ctrl_state == BUMPER_PLAN)
    {

        if (!bumper_planner.finished) //碰撞响应还未结束的情况?
        {
            std::cout << "bumper_planner : running !" << std::endl;
            bumper_planner.run(linear_vel, angular_vel);
        }
        if (bumper_planner.finished)
        {
            if (bumper_planner.lastWarningFlag() == 1)
            {
                if (DataSet::ctrl_state_suspend == NAVI_ACTION && DataSet::ctrl_state_sub_suspend == COVERAGE_PLAN)
                {
                    geometry_msgs::msg::PoseStamped goal_pose;
                    zigzag_planner.IPA_PLAN_PTR()->setNextGoal(goal_pose,
                                                               zigzag_planner.IPA_PLAN_PTR()->path_eps_sqr);
                    navi_planner.setMission(TO_SECTION_POINT, goal_pose);
                    updateState(NAVI_MODE, NAVI_ACTION, NAVI_TO_POSE_PLAN);
                } else
                {
                    std::cout << "bumper_planner: go to small_obs_Planner" << std::endl;
                }
            }
            else if (bumper_planner.lastWarningFlag() == 2)
            {
                updateState(FOLLOW_MODE, OBS_FOLLOW, -1);
            }
            else
            {
                updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                            DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            }
            ZeroSpeed(linear_vel, angular_vel);
            std::cout << "bumper_planner : finished !" << std::endl;
        }
    }
        //-------------------------上盖有障碍物响应程序，依赖硬件，无法验证-------------------------//
    else if (DataSet::ctrl_state == TOP_CAP_OBS_PLAN)
    {
        if (!top_cap_obs_planner.finished) //响应还未结束的情况?
        {
            std::cout << "top_cap_obs : running !" << std::endl;
            top_cap_obs_planner.run(linear_vel, angular_vel);
        }
        if (top_cap_obs_planner.finished)
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "top_cap_obs : finished !" << std::endl;
        }
    }
        //-------------------------阶跃墙面响应程序，依赖硬件，无法验证-------------------------//
    else if (DataSet::ctrl_state == SAWTOOTH_WALL_PLAN)
    {
        if (!sawtooth_wall_planner.finished) //响应还未结束的情况?
        {
            std::cout << "sawtooth_wall : running !" << std::endl;
            sawtooth_wall_planner.state_step = 0;
            sawtooth_wall_planner.run(linear_vel, angular_vel);
        } else
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "sawtooth_wall : finished !" << std::endl;
        }
    }
        //-------------------------倾斜相应程序-------------------------//
    else if (DataSet::ctrl_state == TILT_PLAN)
    {
        if (!tilt_reaction_planner.finished) //响应还未结束的情�?
        {
            std::cout << "tilt_reaction : running !" << std::endl;
            tilt_reaction_planner.run(linear_vel, angular_vel);
        }
        if (tilt_reaction_planner.finished)
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "tilt_reaction : finished !" << std::endl;
        }
    }
        //-------------------------打滑响应程序-------------------------//
    else if (DataSet::ctrl_state == SLIP_PLAN)
    {
        if (!slip_reaction_planner.finished) //响应还未结束的情�?
        {
            std::cout << "slip_reaction : running !" << std::endl;
            slip_reaction_planner.run(linear_vel, angular_vel);
        }

        if (slip_reaction_planner.finished)
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "slip_reaction : finished !" << std::endl;
        }
    }
        //-------------------------小障碍物响应程序-------------------------//
    else if (DataSet::ctrl_state == SMALL_OBS_PLAN)
    {
        if (!small_obs_Planner.finished) //响应还未结束的情�?
        {
            std::cout << "small_obs : running !" << std::endl;
            small_obs_Planner.run(linear_vel, angular_vel);
        }

        if (small_obs_Planner.finished)
        {
            ZeroSpeed(linear_vel, angular_vel);
            if (DataSet::ctrl_state_suspend == NAVI_ACTION && DataSet::ctrl_state_sub_suspend == NAVI_TO_POSE_PLAN)
            {
                while (rclcpp::ok() &&
                       !insert_pose_client.async_send_request(std::string("SmallObsPlanner"), 0, false))
                {
                    std::cout << "small_obs_Planner: start navigation failed, waiting ..." << std::endl;
                }
            }
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "small_obs : finished !" << std::endl;
        }
    }
        //-------------------------位置卡�?�响应程�?-------------------------//
    else if (DataSet::ctrl_state == POSITION_STUCK_PLAN)
    {
        if (!postion_stuck_planner.finished) //响应还未结束的情�?
        {
            std::cout << "postion_stuck : running !" << std::endl;
            postion_stuck_planner.run(linear_vel, angular_vel);
        } else
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "postion_stuck : finished !" << std::endl;
        }
    }
        //-------------------------边刷缠绕响应程序-------------------------//
    else if (DataSet::ctrl_state == BRUSH_WINDER_PLAN)
    {
        if (!brush_winded_planner.finished) //响应还未结束的情�?
        {
            std::cout << "brush_winded : running !" << std::endl;
            brush_winded_planner.run(linear_vel, angular_vel);
        } else
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "brush_winded : finished !" << std::endl;
        }
    }
        //-------------------------风机堵转响应程序-------------------------//
    else if (DataSet::ctrl_state == FAN_BLOCK_PLAN)
    {
        if (!fan_block_planner.finished)
        {
            std::cout << "fan_block : running !" << std::endl;
            fan_block_planner.run(linear_vel, angular_vel);
        }
        else
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "fan_block : finished !" << std::endl;
        }
    }
        //-------------------------�?推动障�?�物响应程序-------------------------//
    else if (DataSet::ctrl_state == FRONT_PUSHABLE_OBS_PLAN)
    {
        if (!front_pushable_obs_planner.finished) //响应还未结束的情�?
        {
            std::cout << "front_pushable_obs : running !" << std::endl;
            front_pushable_obs_planner.run(linear_vel, angular_vel);
        }
        else
        {
            ZeroSpeed(linear_vel, angular_vel);
            updateState(DataSet::clean_mode_suspend, DataSet::ctrl_state_suspend,
                        DataSet::ctrl_state_sub_suspend, -1, -1, -1);
            std::cout << "front_pushable_obs : finished !" << std::endl;
        }
    }
    //-------------------------发布速度 -------------------------//
//    std::cout << "VelPub linear_vel=  " << linear_vel << "  angular_vel= " << angular_vel << std::endl;
    vel_pub_mod.VelPub(linear_vel, angular_vel);
}

void CtrlModule::InstallFunctors()
{
//    this->InstallFunction<evt::Timer2Hz>(&CtrlModule::UpdateTime2Hz);
    this->InstallFunction<evt::Timer100Hz>(&CtrlModule::UpdateTime100Hz);
}

// 这里更新的状态需要获取传感器信息
//void CtrlModule::CtrlStateUpdate()
//{
//    if (DataSet::ctrl_state > ROOM_DIVIDER_PLAN)
//    {
//        bool error_state = false;
//        bool collisionWarning = bumper_planner.CollisionWarning(ROBOT_DIAMETER - 0.01, (ROBOT_RADIUS - 0.02));
//        if ((!bumper_planner.finished && DataSet::bumper_data.GetBit() != 0) ||
//            (DataSet::ctrl_state != SMALL_OBS_PLAN && collisionWarning))
//        {
//            updateState(-1, -1, -1,
//                    DataSet::clean_mode, DataSet::ctrl_state, DataSet::ctrl_state_sub);
//            error_state = true;
//            DataSet::ctrl_state = BUMPER_PLAN;
//            bumper_planner.finished = false;
//            cliff_follow_planner.SetFinishTrueClearPose();
//        }
//        std::cout << "ctrl_state: " << DataSet::ctrl_state
//                  << " clean_mode: " << DataSet::clean_mode
//                  << " error_state: " << error_state << std::endl;
//        if (!error_state)
//        {
//            if (DataSet::clean_mode == FOLLOW_MODE)
//            {
//                DataSet::ctrl_state = OBS_FOLLOW;
//            }
//            else if (DataSet::clean_mode == NAVI_MODE)
//            {
//                DataSet::ctrl_state = NAVI_ACTION;
//            }
//        }
//        MonitorUpdate();
//    }
//    //-------------------------self test 开�?-------------------------//
//    self_test_monitor.run();
//}

void CtrlModule::CtrlStateUpdate()
{
    if (DataSet::ctrl_state > ROOM_DIVIDER_PLAN)
    {
        bool error_state = false;
        if (DataSet::cliff_data.GetCliffBit() != 0 || !cliff_follow_planner.finished ||
            !cliff_reaction_planner.finished)
        {
            error_state = true;
            if (!DataSet::off_ground_switch.GetData())  //dpj
            {
                std::cout << "------------- cliff -------------" << std::endl;
                cliff_follow_planner.SetStartPose();
                if (DataSet::clean_mode == FOLLOW_MODE ||
                    (DataSet::clean_mode == NAVI_MODE && DataSet::ctrl_state_sub == COVERAGE_PLAN))
                {
                    DataSet::ctrl_state = CLIFF_FOLLOW;
                    cliff_follow_planner.finished = false; //进入沿悬崖模式前先将标志位置为false，当其他�?触发后置为true，�?�Bumper、�?�感
                }
                else if (DataSet::clean_mode == NAVI_MODE && DataSet::ctrl_state_sub == NAVI_TO_POSE_PLAN)
                {
                    if (DataSet::ctrl_state == NAVI_ACTION)
                        if (insert_pose_client.async_send_request(
                                std::string("CliffReactionPlanner"), 0.2, true))
                            std::cout << "CliffReactionPlanner: pause navigation success!" << std::endl;
                    updateState(-1, -1, -1,
                            DataSet::clean_mode, DataSet::ctrl_state, DataSet::ctrl_state_sub);
                    DataSet::ctrl_state = CLIFF_ACTION;
                    cliff_reaction_planner.finished = false;
                }
            }
        }
        else if (!error_state)
        {
            if (DataSet::clean_mode == FOLLOW_MODE)
            {
                DataSet::ctrl_state = OBS_FOLLOW;
            }
            else if (DataSet::clean_mode == NAVI_MODE)
            {
                DataSet::ctrl_state = NAVI_ACTION;
            }
        }

        if (DataSet::magnetic_data.GetBit() != 0 || !virtual_obs_follow_planner.finished)
        {
            std::cout << "--------- virtual_obs ---------" << std::endl;
            error_state = true;
            DataSet::ctrl_state = BUMPER_PLAN;
            virtual_obs_follow_planner.finished = false;
            cliff_follow_planner.SetFinishTrueClearPose();
        }
        else if (!error_state)
        {
            if (DataSet::clean_mode == FOLLOW_MODE)
            {
                DataSet::ctrl_state = OBS_FOLLOW;
            }
            else if (DataSet::clean_mode == NAVI_MODE)
            {
                DataSet::ctrl_state = NAVI_ACTION;
            }
        }


        bool collisionWarning = bumper_planner.CollisionWarning(ROBOT_DIAMETER - 0.01, (ROBOT_RADIUS - 0.02));
        if ((!bumper_planner.finished && DataSet::bumper_data.GetBit() != 0) ||
            (DataSet::ctrl_state != SMALL_OBS_PLAN && collisionWarning))
        {
            std::cout << "--------- bumper ---------" << std::endl;
            error_state = true;
            DataSet::ctrl_state = BUMPER_PLAN;
            bumper_planner.finished = false;
            cliff_follow_planner.SetFinishTrueClearPose();
        }
        else if (!error_state)
        {
            std::cout << "clean_mode: "<< DataSet::clean_mode
                      << " ctrl_state: "<< DataSet::ctrl_state
                      << " FOLLOW_MODE: "<< FOLLOW_MODE << std::endl;
            if (DataSet::clean_mode == FOLLOW_MODE)
            {
                DataSet::ctrl_state = OBS_FOLLOW;
            }
            else if (DataSet::clean_mode == NAVI_MODE)
            {
                DataSet::ctrl_state = NAVI_ACTION;
            }
        }
//        MonitorUpdate();
    }
    //-------------------------self test 开�?-------------------------//
    self_test_monitor.run();
}


void CtrlModule::MonitorUpdate()
{
    std::cout << "--------- MonitorUpdate --------" << std::endl;
    //LogPub::Info("behavior_controller", "[MonitorModule] UpdateTime100Hz");
    //所有的检测器都将写在这里，进行轮�?检�?
    //-------------------------�?空�?�测开�?-------------------------//
    //vacant_monitor.run();
    //std::cout<< "vacant_monitor";
    //-------------------------打滑检测开�?-------------------------//
    slip_monitor.run();
    std::cout<< "slip_monitor " ;
    //-------------------------位置卡�?��?�测开�?-------------------------//
    postion_stuck_monitor.run();
    std::cout<< "postion_stuck_monitor " ;
    //-------------------------边刷缠绕检测开�?-------------------------//
    //brush_winded_monitor.run();
//    std::cout<< "brush_winded_monitor " ;
    //-------------------------风机堵转检测开�?-------------------------//
    //fan_block_monitor.run();
//    std::cout<< "fan_block_monitor " ;
    //-------------------------阶跃的�?�面检测开�?-------------------------//
    sawtooth_wall_monitor.run();
    std::cout<< "sawtooth_wall_monitor " ;
    //-------------------------�?推动障�?�物检测开�?-------------------------//
    front_pushable_obs_monitor.run();
    std::cout<< "front_pushable_obs_monitor " ;
    //-------------------------上盖障�?��?�测开�?-------------------------//
    top_cap_obs_monitor.run();
    std::cout<< "top_cap_obs_monitor " ;
    //-------------------------倾�?��?�测开�?-------------------------//
    tilt_monitor.run();
    std::cout<< "tilt_monitor " ;
    //-------------------------小障碍物检测开�?-------------------------//
    small_obs_monitor.run();
    std::cout << "small_obs_monitor!!!" << std::endl;
}

} // namespace behavior_controller
